#include "quat.hpp"

#include <math.h>
#include <string.h>
#include <iostream>

Quaternion::Quaternion() :
    w(1), x(0), y(0), z(0) {
}

Quaternion::Quaternion(float _w, float _x, float _y, float _z) {
    w = _w;
    x = _x;
    y = _y;
    z = _z;
}

Quaternion::Quaternion(const Vector &v, float _w) :
    w(_w), x(v.x), y(v.y), z(v.z) {
}

Quaternion::Quaternion(float theta, const Vector &axis) {
    float a = sin(theta/2);

    w = cos(theta/2);
    x = axis.x * a;
    y = axis.y * a;
    z = axis.z * a;
}

void Quaternion::normalize() {
    float l = sqrt( w*w + x*x + y*y + z*z );
    w /= l;
    x /= l;
    y /= l;
    z /= l;
}

void Quaternion::set_yaw_pitch_roll(float yaw, float pitch, float roll) {
    float sinp = sin(pitch/2);
    float siny = sin(yaw/2);
    float sinr = sin(roll/2);
    float cosp = cos(pitch/2);
    float cosy = cos(yaw/2);
    float cosr = cos(roll/2);
    
    w = cosr * cosp * cosy + sinr * sinp * siny;
    x = sinr * cosp * cosy - cosr * sinp * siny;
    y = cosr * sinp * cosy + sinr * cosp * siny;
    z = cosr * cosp * siny - sinr * sinp * cosy;
    
    normalize();
}

Quaternion& Quaternion::operator*=(const Quaternion &q) { 
    w = w * q.w - x * q.x - y * q.y - z * q.z;
    x = w * q.x + x * q.w + y * q.z - z * q.y;
    y = w * q.y + y * q.w + z * q.x - x * q.z;
    z = w * q.z + z * q.w + x * q.y - y * q.x;

    return *this;
}

Quaternion Quaternion::operator* (const Quaternion &q) const { 
    return Quaternion(w * q.w - x * q.x - y * q.y - z * q.z,
                      w * q.x + x * q.w + y * q.z - z * q.y, 
                      w * q.y + y * q.w + z * q.x - x * q.z,
                      w * q.z + z * q.w + x * q.y - y * q.x);
}


void Quaternion::get_matrix(float m[16]) const {
    float x2 = x * x;
    float y2 = y * y;
    float z2 = z * z;
    float xy = x * y;
    float xz = x * z;
    float yz = y * z;
    float wx = w * -x;
    float wy = w * -y;
    float wz = w * -z;

    memset(m, 0, sizeof(m));
    
    m[0] = 1.0f - 2.0f * (y2 + z2);
    m[1] = 2.0f * (xy - wz);
    m[2] = 2.0f * (xz + wy);
    //m[3] = 0.0f;
    m[4] = 2.0f * (xy + wz);
    m[5] = 1.0f - 2.0f * (x2 + z2);
    m[6] = 2.0f * (yz - wx);
    //m[7] = 0.0f;
    m[8] = 2.0f * (xz - wy);
    m[9] = 2.0f * (yz + wx);
    m[10] = 1.0f - 2.0f * (x2 + y2);
    //m[11] = 0.0f;
    //m[12] = 0.0f;
    //m[13] = 0.0f;
    //m[14] = 0.0f;
    m[15] = 1.0f;
}

std::ostream& operator <<(std::ostream &o, const Quaternion &q) {
    o << "(" << q.w << ", " << q.x << ", " << q.y << ", " << q.z << ")";
    return o;
}
